/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package us.oh.k12.wkw.robot.command;

import edu.wpi.first.wpilibj.command.WaitCommand;

/**
 *
 * @author Team4145
 */
public class AutonomousBehindToShootRight extends CommandGroupBase{
    
    public AutonomousBehindToShootRight() {

		super("AutonomousBehindToShootRight");

                try {
                    
                this.addSequential(new DriveStraightCmd(1));
                this.addSequential(new DriveTurn90Cmd(false));
                this.addSequential(new DriveStraightCmd(1));
                this.addSequential(new DriveStopCmd());
                this.addSequential(new LauncherAngleToFeedCmd());
                 
                this.addSequential(new WaitCommand(1.5));
                
                this.addSequential(new DriveXPidEnableCmd());
                this.addSequential(new LauncherToggleAnglePidCmd());
                       
                this.addSequential(new WaitCommand(3));
                this.addSequential(new DriveXPidDisableCmd());
                
                    
		this.addSequential(new StopCompressorCmd());
                
                
                this.addSequential(new LauncherAnglePidDisableCmd());
                this.addSequential(new LauncherAngleMotorHoldCmd());
		
		this.addSequential(new LauncherMotorStartCmd());
              
		this.addSequential(new WaitCommand(3.1));
		this.addSequential(new LauncherPlungerRetractCmd());
                this.addSequential(new WaitCommand(.2));
                this.addSequential(new LauncherPlungerFireCmd());
                this.addSequential(new LauncherFeedFrisbeeCmd());
                this.addSequential(new WaitCommand(.8));
                this.addSequential(new LauncherPlungerRetractCmd());
                this.addSequential(new WaitCommand(.2));
                this.addSequential(new LauncherPlungerFireCmd());
                this.addSequential(new LauncherFeedFrisbeeCmd());
                this.addSequential(new WaitCommand(.8));
                this.addSequential(new LauncherPlungerRetractCmd());
                this.addSequential(new WaitCommand(.2));
                this.addSequential(new LauncherPlungerFireCmd());
             
                
		this.addSequential(new LauncherMotorStopCmd());
                
                this.addSequential(new StartCompressorCmd());
                this.addSequential(new LauncherAngleStopCmd());
                
               
                

	}   catch (Exception anEx) {
			this.error("AutonomousBehindToShootRight()", anEx);
		}
        }
    
}
